package br.ufmg.dees.insane.model.geo.computationalGeometry;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.Iterator;

import br.ufmg.dees.insane.model.geo.halfEdgeDataStructure.Vertex;
import br.ufmg.dees.insane.util.IMatrix;
import br.ufmg.dees.insane.util.IPoint3d;
import br.ufmg.dees.insane.util.IVector;

/**
 * 
 * @author Samuel Silva Penna
 * @since 17/10/2006
 */
public final class ComputationalGeometry{
	
    
    
	/**
	 * The method sort by coord x the set of vertices passade by parameters 
	 * @param vertices
	 * @return sort The vertices list sorted
	 */
	public static ArrayList<Vertex> sortBySelectionX(ArrayList<Vertex> vertices){
        ArrayList<Vertex> sort = new ArrayList<Vertex>();
		ArrayList auxList = vertices;
		Vertex v = (Vertex)vertices.get(0);
		Iterator it = vertices.iterator();
		while(it.hasNext()){
			v = (Vertex)it.next();
			Iterator itInt = auxList.iterator();
			while(itInt.hasNext()){
				Vertex cur = (Vertex)itInt.next();
				if(v.getCoords().x>cur.getCoords().x){
					v = cur;
				}else if(v.getCoords().x==cur.getCoords().x){
					
					if(v.getCoords().y>cur.getCoords().y){
						v = cur;
					} 
				}
			}
			auxList.remove(v);
			it = auxList.listIterator();
			sort.add(v);
		}
		return sort;
	}

	/**
	 * The method sort by coord y the set of vertices passade by parameters 
	 * @param vertices
	 * @return sort The vertices list sorted
	 */
	public static ArrayList<Vertex> sortBySelectionY(ArrayList<Vertex> vertices){
		ArrayList<Vertex> sort = new ArrayList<Vertex>();
		ArrayList auxList = vertices;
		Vertex v = (Vertex)vertices.get(0);
		Iterator it = vertices.iterator();
		while(it.hasNext()){
			v = (Vertex)it.next();
			Iterator itInt = auxList.iterator();
			while(itInt.hasNext()){
				Vertex cur = (Vertex)itInt.next();
				if(v.getCoords().y>cur.getCoords().y){
					v = cur;
				}else if(v.getCoords().y==cur.getCoords().y){
					
					if(v.getCoords().x<cur.getCoords().x){
						v = cur;
					} 
				}
			}
			auxList.remove(v);
			it = auxList.listIterator();
			sort.add(v);
		}
		return sort;
	}
	
	/**
	 * The method sort by coord y the set of vertices passade by parameters 
	 * @param vertices
	 * @return sort The vertices list sorted
	 */
	public static ArrayList<Vertex> polarSort(Vertex ov, ArrayList<Vertex> vertices){
		ArrayList<Vertex> sort = new ArrayList<Vertex>();
		//ArrayList<Vertex> auxList = vertices;
		Iterator it = vertices.iterator();
		while(it.hasNext()){
			if(vertices.size()!=1){
			Vertex v = ComputationalGeometry.getMinAngle(ov,ComputationalGeometry.relativeAngles(ov,vertices));
			sort.add(v);
			vertices.remove(v);
			it = vertices.iterator();
			}else{
				break;
			}
		}
		return sort;
	}
	
	public static HashMap relativeAngles(Vertex ov,ArrayList vertices){
		HashMap<Object, Object> relativeAngles = new HashMap<Object, Object>();
		Vertex H = new Vertex("H",new IPoint3d(ov.getCoords().x+1,ov.getCoords().y));
		for(int i=0;i<vertices.size();i++){
			if(!ov.equals(vertices.get(i))){
				Vertex v = (Vertex)vertices.get(i);
				relativeAngles.put(vertices.get(i),angle(H,ov,v));
			}
		}
		return relativeAngles;
	}
	
	public static Vertex getMinAngle(Vertex ov,HashMap map){
		Vertex v = new Vertex();
		
		double angleCur = (Double)map.get(map.keySet().toArray()[0]);
		Vertex vCur = (Vertex)map.keySet().toArray()[0];
		for(int i=0;i<map.size();i++){
			if(vCur!= (Vertex)map.keySet().toArray()[i]){
				
				if(angleCur>(Double)map.get(map.keySet().toArray()[i])){
					angleCur = (Double)map.get(map.keySet().toArray()[i]);
					vCur = (Vertex)map.keySet().toArray()[i];
				}else if(angleCur==(Double)map.get(map.keySet().toArray()[i])){
					if(ov.getCoords().distanceSquared(vCur.getCoords())>ov.getCoords().distanceSquared(((Vertex)map.keySet().toArray()[i]).getCoords())){
						continue;
					}else{
						angleCur = (Double)map.get(map.keySet().toArray()[i]);
						vCur = (Vertex)map.keySet().toArray()[i];
					}
				}
			}
		}
		v = vCur;
		return v;
	}
	
	
	public static double[] sortByInsertion( double[] arrElements ){
		double key;
    	int i;

       for( int j=1; j<arrElements.length; j++ )
       {
          key = arrElements[j];

          // Para ordem decrescente coloque arrElements[i]<key
          for( i=j-1; i>=0 && arrElements[i]>key; i-- )
             arrElements[i+1] = arrElements[i];

          arrElements[i+1]= key;
       }
       
       return arrElements;
    }
	    
	/**
	 * The method return the relative angle between two vectors passed by parameters.
	 * @param v1 First vector.
	 * @param v2 Second vector
	 * @return angle The relative angle between two vectors.
	 */
	public static double angle(IVector v1,IVector v2){
		if(v2.getElement(1)>=0){
			double dot = v1.dot(v2);
			double normV1 = v1.norm();
			double normV2 = v2.norm();
			double angle = Math.toDegrees(Math.acos(dot/(normV1*normV2)));
			return angle;
		}else{
			double dot = v1.dot(v2);
			double normV1 = v1.norm();
			double normV2 = v2.norm();
			double angle = Math.toDegrees(Math.acos(dot/(normV1*normV2)));
			return 360-angle;
		}
		
	}
	
	
	public static double angle(Vertex vrt1,Vertex vrt2,Vertex vrt3){
		double[] vector1 = {(vrt1.getCoords().x-vrt2.getCoords().x),
				(vrt1.getCoords().y-vrt2.getCoords().y)};
		IVector v1 = new IVector(vector1);
		double[] vector2 = {(vrt3.getCoords().x-vrt2.getCoords().x),
				(vrt3.getCoords().y-vrt2.getCoords().y)};
		IVector v2 = new IVector(vector2);
		if(v2.getElement(1)>=0){
			double dot = v1.dot(v2);
			double normV1 = v1.norm();
			double normV2 = v2.norm();
			double angle = Math.toDegrees(Math.acos(dot/(normV1*normV2)));
			return angle;
		}else{
			double dot = v1.dot(v2);
			double normV1 = v1.norm();
			double normV2 = v2.norm();
			double angle = Math.toDegrees(Math.acos(dot/(normV1*normV2)));
			return 360-angle;
		}
	}
	
	/**
	 * The method return the pseudoAngle between two vectors passed by parameters.
	 * @param v1 First vector.
	 * @param v2 Second vector
	 * @return pseudoAngle The pseudoAngle between two vectors.
	 */
	public static double pseudoAngle(IVector v1,IVector v2){
		if(v2.getElement(1)>=0){
			double dot = v1.dot(v2);
			double normV1 = v1.norm();
			double normV2 = v2.norm();
			double pseudoAngle = 1-(dot/(normV1*normV2));
			return pseudoAngle;
		}else{
			double dot = v1.dot(v2);
			double normV1 = v1.norm();
			double normV2 = v2.norm();
			double pseudoAngle = 1-(dot/(normV1*normV2));
			return 4-pseudoAngle;
		}
	}
	
	/**
	 * @param vrt1
	 * @param vrt2
	 * @param vrt3
	 * @return the pseudoangle
	 */
	public static double pseudoAngle(Vertex vrt1,Vertex vrt2,Vertex vrt3){
		double[] vector1 = {(vrt1.getCoords().x-vrt2.getCoords().x),
				(vrt1.getCoords().y-vrt2.getCoords().y)};
		IVector v1 = new IVector(vector1);
		double[] vector2 = {(vrt3.getCoords().x-vrt2.getCoords().x),
				(vrt3.getCoords().y-vrt2.getCoords().y)};
		IVector v2 = new IVector(vector2);
		if(v2.getElement(1)>=0){
			double dot = v1.dot(v2);
			double normV1 = v1.norm();
			double normV2 = v2.norm();
			double pseudoAngle = 1-(dot/(normV1*normV2));
			return pseudoAngle;
		}else{
			double dot = v1.dot(v2);
			double normV1 = v1.norm();
			double normV2 = v2.norm();
			double pseudoAngle = 1-(dot/(normV1*normV2));
			return 4-pseudoAngle;
		}
	}
	
	
	/**
	 * The method return the Barycentre  of a set of vertices.
	 * @param vertices The set of vertices
	 * @return vertex The vertex of Barycentre .
	 */
	public static Vertex barycentre(ArrayList vertices){
		double x = 0.0;
		double y = 0.0;
		for(int i=0;i<vertices.size();i++){
			x+=((Vertex)vertices.get(i)).getCoords().x;
		}
		for(int i=0;i<vertices.size();i++){
			y+=((Vertex)vertices.get(i)).getCoords().y;
		}
		Vertex o = new Vertex("o", new IPoint3d(x/vertices.size(),y/vertices.size()));
		return o;
	}
		
	
	public static ArrayList<Vertex> convexHullJarvis(ArrayList<Vertex> vertices){
		ArrayList<Vertex> convex = new ArrayList<Vertex>(); 
		//ArrayList verticesList = vertices;
		ArrayList sortY = sortBySelectionY(vertices);
		Vertex o = (Vertex)sortY.get(0);
		convex.add(o);
		
		//ArrayList angles = new ArrayList();
		Vertex vHull = new Vertex(); 
		for(int i=1;i<sortY.size()-1;i++){
			Vertex next = (Vertex)sortY.get(i);
			vHull = next;
			double angle = angle(new Vertex("H",new IPoint3d(o.getCoords().x+1,o.getCoords().y)),o,next);			
			
			for(int j=0;j<sortY.size();j++){
				if(!convex.contains(next)&&sortY.get(j)!=next){
					if(angle>=angle(new Vertex("H",new IPoint3d(o.getCoords().x+1,o.getCoords().y)),o,(Vertex)sortY.get(j))){
						angle = angle(new Vertex("H",new IPoint3d(o.getCoords().x+1,o.getCoords().y)),o,(Vertex)sortY.get(j));
						vHull = (Vertex)sortY.get(j);
					}
				}
			}
			
			convex.add(vHull);
			o = vHull;
		}
		
		return convex;
	}	
	
	public static ArrayList convexHullGraham(ArrayList<Vertex> points) {
		points = sortBySelectionY(points);
		Vertex first = (Vertex)points.get(0);
		points = polarSort(first,points);
		if (points.size() >= 3) {
	    	ArrayList<Vertex> C = new ArrayList<Vertex>();
	        C.add(points.get(0));
	        C.add(points.get(1));

	        int qm = 0;
	        int qn = 1;
	        int k = 2;
	        for (; k < points.size(); k++) {
	            double result = crossProduct((Vertex)C.get(qm),(Vertex)C.get(qn),(Vertex)points.get(k));
	            while (result <= 0) {
	                if (qm != 0) {
	                    C.remove(qn);
	                    qn--; qm--;
	                    result = crossProduct((Vertex)C.get(qm),(Vertex)C.get(qn),(Vertex)points.get(k));
	                }
	                else
	                    break;
	            }
	                
	            C.add((Vertex)points.get(k));
	            qm = qn; qn++;
	        }
	            
	        double result = crossProduct((Vertex)C.get(qm),(Vertex)C.get(qn),(Vertex)points.get(0));
	        while (result <= 0) {
	            if (qm != 0) {
	                C.remove(qn);
	                qn--; qm--;
	                result = crossProduct((Vertex)C.get(qm),(Vertex)C.get(qn),(Vertex)points.get(0));
	            }
	            else
	                break;
	        }
	        C.add(0,first);
	        return C;
	    }
	    else
	        return new ArrayList();
	}

	
	/**
	 * The method return the result of a crossproduct between two vector p1p2 and p2p3 composed
	 * by three vertices p1, p2 and p3.
	 * @param p1 The vertex p1.
	 * @param p2 The vertex p2.
	 * @param p3 The vertex p3.
	 * @return crossproduct The result of the crossproduct.
	 */
	public static double crossProduct(Vertex p1, Vertex p2, Vertex p3) { 
    	IMatrix m = new IMatrix(3,3);
    	m.setElement(0,0,p1.getCoords().x); m.setElement(0,1,p1.getCoords().y); m.setElement(0,2,1);
    	m.setElement(1,0,p2.getCoords().x); m.setElement(1,1,p2.getCoords().y); m.setElement(1,2,1);
    	m.setElement(2,0,p3.getCoords().x); m.setElement(2,1,p3.getCoords().y); m.setElement(2,2,1);
    	return m.determinantLU();
    }
	
	
	public static void main(String[] args){
		
		Vertex v1 = new Vertex("1", new IPoint3d(1,1));
		Vertex v2 = new Vertex("2", new IPoint3d(7,3));
		Vertex v3 = new Vertex("3", new IPoint3d(4,4));
		Vertex v4 = new Vertex("4", new IPoint3d(3.5,1));
		Vertex v5 = new Vertex("5", new IPoint3d(4,5));
		Vertex v6 = new Vertex("6", new IPoint3d(10,10));
		Vertex v7 = new Vertex("7", new IPoint3d(10,10));
		ArrayList<Vertex> list = new ArrayList<Vertex>();
		list.add(v1);
		list.add(v2);
		list.add(v3);
		list.add(v4);
		list.add(v5);
		list.add(v6);
		list.add(v7);
		
		ArrayList auxList = ComputationalGeometry.sortBySelectionX(list);
		
		for(int i=0;i<auxList.size();i++){
			System.out.println("X: vertice "+((Vertex)auxList.get(i)).getId());
		}
		System.out.println("******************");
		
		list.add(v1);
		list.add(v2);
		list.add(v3);
		list.add(v4);
		list.add(v5);
		list.add(v6);
		list.add(v7);
		auxList = ComputationalGeometry.sortBySelectionY(list);
		for(int i=0;i<auxList.size();i++){
			System.out.println("Y: vertice "+((Vertex)auxList.get(i)).getId());
		}
		System.out.println("******************");
		
		v1 = new Vertex("1", new IPoint3d(0,0));
		v2 = new Vertex("2", new IPoint3d(1,0));
		v3 = new Vertex("3", new IPoint3d(1,1));
		v4 = new Vertex("4", new IPoint3d(0,1));
		list = new ArrayList<Vertex>();
		list.add(v1);
		list.add(v2);
		list.add(v3);
		list.add(v4);
		Vertex o = ComputationalGeometry.barycentre(list);
		System.out.println("Ox "+o.getCoords().x);
		System.out.println("Oy "+o.getCoords().y);
		System.out.println("******************");
		
		double[] vector1 = {1,0};
		IVector vc1 = new IVector(vector1);
		double[] vector2 = {0,1};
		IVector vc2 = new IVector(vector2);
		double pseudo = ComputationalGeometry.pseudoAngle(vc1,vc2);
		System.out.println("PseudoVector "+pseudo);
		System.out.println("******************");
		
		v1 = new Vertex("1", new IPoint3d(1,0));
		 v2 = new Vertex("2", new IPoint3d(0,0));
		 v3 = new Vertex("3", new IPoint3d(0,-1));
		pseudo = ComputationalGeometry.pseudoAngle(v1,v2,v3);
		System.out.println("Pseudo "+pseudo);
		System.out.println("******************");
		
		double[] vectorA1 = {1,0};
		vc1 = new IVector(vectorA1);
		double[] vectorA2 = {0,-1};
		vc2 = new IVector(vectorA2);
		double angle = ComputationalGeometry.angle(vc1,vc2);
		System.out.println("AngleVector "+angle);
		System.out.println("******************");
		
		v1 = new Vertex("1", new IPoint3d(1.5+1,1.5));
		v2 = new Vertex("2", new IPoint3d(1.5,1.5));
		v3 = new Vertex("3", new IPoint3d(1,1));
		angle = ComputationalGeometry.angle(v1,v2,v3);
		System.out.println("Angle "+angle);
		System.out.println("******************");
		
		 v1 = new Vertex("1", new IPoint3d(1,1));
		 v2 = new Vertex("2", new IPoint3d(2,1));
		 v3 = new Vertex("3", new IPoint3d(2,2));
		 v4 = new Vertex("4", new IPoint3d(1,2));
		 v5 = new Vertex("5", new IPoint3d(1.5,1.5));
		 v6 = new Vertex("6", new IPoint3d(0.5,1.5));
		 v7 = new Vertex("7", new IPoint3d(1,1.5));
		 list = new ArrayList<Vertex>();
		 list.add(v7);
		 list.add(v4);
		 list.add(v3);
		 list.add(v2);
		 list.add(v5);
		 list.add(v6);
		 list.add(v1);
		 auxList = new ArrayList();
		// Vertex v = ComputationalGeometry.getMinAngle(v1,ComputationalGeometry.relativeAngles(v1,list));
		 
		 
		 auxList = ComputationalGeometry.polarSort(v1,list);
		 
		for(int i =0;i<auxList.size();i++){
			System.out.println("Vertice "+((Vertex)auxList.get(i)).getId());
		}
		// System.out.println("Vertice "+v.getId());
		 
		
		System.out.println("******************");
		
		 v1 = new Vertex("1", new IPoint3d(1,1));
		 v2 = new Vertex("2", new IPoint3d(2,1));
		 v3 = new Vertex("3", new IPoint3d(2,2));
		 v4 = new Vertex("4", new IPoint3d(1,2));
		 v5 = new Vertex("5", new IPoint3d(1.6,1.5));
		 v6 = new Vertex("6", new IPoint3d(0.5,1.5));
		 v7 = new Vertex("7", new IPoint3d(1,1.5));
		 list = new ArrayList<Vertex>();
		 list.add(v7);
		 list.add(v4);
		 list.add(v3);
		 list.add(v2);
		 list.add(v5);
		 list.add(v6);
		 list.add(v1);
		 auxList = new ArrayList();
		 
		 auxList = ComputationalGeometry.convexHullGraham(list);
		 
		for(int i =0;i<auxList.size();i++){
			System.out.println("Vertice "+((Vertex)auxList.get(i)).getId());
		}
		// System.out.println("Vertice "+v.getId());
		
	}
	
	
	
//******************************************************************	
}